#include <memory>
#include <vector>
#include <string>
#include <cmath>
#include <optional>
#include "rclcpp/rclcpp.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
#include "builtin_interfaces/msg/duration.hpp"
#include "std_srvs/srv/trigger.hpp"
#include "rclcpp/parameter_event_handler.hpp"

//查看关节角度的的接口
#include "sensor_msgs/msg/joint_state.hpp"

// 自己服务接口文件
#include "virtual_arm_interfaces/srv/set_gripper.hpp"

using JointTrajectory = trajectory_msgs::msg::JointTrajectory;
using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint;
using SetGripper = virtual_arm_interfaces::srv::SetGripper;
using Trigger = std_srvs::srv::Trigger;
using JointState = sensor_msgs::msg::JointState;

class GripperServer : public rclcpp::Node{
private:
  rclcpp::Publisher<JointTrajectory>::SharedPtr host_publisher_,follow_publisher_ ; //主动和随动夹爪的position发布者
  rclcpp::Service<SetGripper>::SharedPtr set_position_service_; // 设置夹爪position的服务
  rclcpp::Service<Trigger>::SharedPtr open_service_, close_service_; // 方便调试的默认开关夹爪服务
  OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; // 添加参数更改回调函数返回句柄
  rclcpp::Subscription<JointState>::SharedPtr joint_subscripter_; // joint_position 话题订阅者，用于获取当前角度
  rclcpp::TimerBase::SharedPtr init_postion_timer_; // 初始化角度一次性定时器
  bool got_joint_state_once_{false}; // 是否收到过订阅的JointState的信息判断
  double joint5_position{0.0},joint6_right_position{0.0}; // position 用来接收jointstate订阅返回的角度

  // 参数事件监听
  std::shared_ptr<rclcpp::ParameterEventHandler> param_event_handler_;
  rclcpp::ParameterCallbackHandle::SharedPtr cb_j5_,cb_j6_;

  // 待应用的命令缓存
  std::optional<double> pending_j5_;
  std::optional<double> pending_j6_;

  // 更改joint_position参数的一次性定时器
  rclcpp::TimerBase::SharedPtr joint_param_apply_once_timer_;

  // 把服务和定时器放在同一个互斥回调组，防止并发踩状态
  rclcpp::CallbackGroup::SharedPtr cbgroup_one_;

public:
  GripperServer():Node("gripper_server"){
    // 设置参数
    declare_parameter<std::vector<std::string>>("host_joint_names", {"joint5","joint6_right"}); //主动关节名称
    declare_parameter<std::vector<std::string>>("follow_joint_names",
      {"joint7_right","joint8_right","joint9_left","joint10_left","joint11_left"}); //被动关节名称
    declare_parameter<double>("joint5_position",0.0);
    declare_parameter<double>("joint6_right_position",0.0);
    declare_parameter<std::vector<double>>("follower_scale", {1.0,1.0,1.0,1.0,1.0}); //随动角度乘法因子（可选拓展）
    declare_parameter<std::vector<double>>("follower_offset",{0.0,0.0,0.0,0.0,0.0});  // 随动角度偏移量（可选拓展）
    declare_parameter<double>("move_time", 1.0); // 运动历经时间
    declare_parameter<double>("start_delay", 0.2); // 为了使得被动关节与主动关节同步，设置的开始时间的延时
    declare_parameter<double>("open_angle", 0.7); // 默认开启夹爪的角度
    declare_parameter<double>("close_angle", 0.0); //默认关闭夹爪的角度

    // 创建互斥回调组，让服务和定时器串行进行，避免共享状态抢占
    this->cbgroup_one_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);


    // Qos 用来定义消息传输的规则 有很多历史策略、可靠性。。。
    rclcpp::QoS qos(rclcpp::KeepLast(1)); //历史策略：只保留最新的一条信息
    qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); //可靠性：必须可靠

    //主动关节发布者
    this->host_publisher_ = this->create_publisher<JointTrajectory>("/host_gripper_controller/joint_trajectory",qos);
    //被动关节发布者
    this->follow_publisher_ = this->create_publisher<JointTrajectory>("/follow_gripper_controller/joint_trajectory",qos);

    //设置position
    this->set_position_service_ = this->create_service<SetGripper>("/gripper/set",std::bind(&GripperServer::SetGripperPosition,this,std::placeholders::_1,std::placeholders::_2),rmw_qos_profile_services_default,cbgroup_one_);
    //默认开启和关闭服务
    this->open_service_ = this->create_service<Trigger>("/gripper/open",std::bind(&GripperServer::OpenGripper,this,std::placeholders::_1,std::placeholders::_2),rmw_qos_profile_services_default,cbgroup_one_);
    this->close_service_ = this->create_service<Trigger>("/gripper/close",std::bind(&GripperServer::CloseGripper,this,std::placeholders::_1,std::placeholders::_2),rmw_qos_profile_services_default,cbgroup_one_);

    // 参数更改回调
    this->parameter_callback_handle_ = this->add_on_set_parameters_callback(std::bind(&GripperServer::paramCallback,this,std::placeholders::_1));

    // 指定的joint_position参数改变事件回调（异步）
    this->param_event_handler_ = std::make_shared<rclcpp::ParameterEventHandler>(this);
    
    // 监听joint5_position
    this->cb_j5_ = this->param_event_handler_->add_parameter_callback("joint5_position",
    [this](const rclcpp::Parameter & p){
      if (p.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) {
        this->request_publish_from_params(p.as_double(), std::nullopt);
      } else {
        RCLCPP_WARN(this->get_logger(),
          "joint5_position expects double, got %s", p.get_type_name().c_str());
      }
    });

    // 监听joint6_right_position
    this->cb_j6_ = this->param_event_handler_->add_parameter_callback("joint6_right_position",
      [this](const rclcpp::Parameter & p){
        if (p.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) {
          this->request_publish_from_params(std::nullopt, p.as_double());
        } else {
          RCLCPP_WARN(this->get_logger(),
            "joint6_right_position expects double, got %s", p.get_type_name().c_str());
        }
      }
    );


    // 订阅JointState
    this->joint_subscripter_ = this->create_subscription<JointState>("joint_states",rclcpp::QoS(rclcpp::KeepLast(1)).reliable(),
    [this](JointState::SharedPtr msg){
      this->joint_listener_callback(msg);
      got_joint_state_once_ = true;
    });

    RCLCPP_INFO(get_logger(), "gripper_server ready. Services: /gripper/set, /gripper/open, /gripper/close");

    //通过一次性定时器，等待系统就绪后在发布初始化轨迹
    using namespace std::chrono_literals;
    this->init_postion_timer_ = this->create_wall_timer(100ms,std::bind(&GripperServer::init_position_callback,this));



  }



  //获取夹爪角度
  double get_joint5_position(){
    return this->joint5_position;
  }

  double get_joint6_right_position(){
    return this->joint6_right_position;
  }

  std::vector<std::pair<std::string,double>> get_gripper_position(){
    return {
      {"joint5", this->joint5_position},
      {"joint6_right", this->joint6_right_position}
    };
  }

private:
  // joint_position 参数更改统一入口，负责记录更改的position + 触发一次定时器
  void request_publish_from_params(std::optional<double> j5,std::optional<double> j6){
    if (j5) this->pending_j5_ = *j5;
    if (j6) this->pending_j6_ = *j6;
    schedule_apply_once(std::chrono::milliseconds(1));
  }

  // 创建/重置一次行定时器， 定时回调中完成就绪检查与真正的publish
  void schedule_apply_once(std::chrono::milliseconds delay){

    // 如果旧定时器存在，清除它
    if(this->joint_param_apply_once_timer_)this->joint_param_apply_once_timer_->cancel();

    // 建立新的一次性定时器
    this->joint_param_apply_once_timer_ = this->create_wall_timer(delay,[this](){
      // 判断是否有参数更新
      if(!this->pending_j5_ && !this->pending_j6_){
        this->joint_param_apply_once_timer_->cancel();
        return ;
      }

      //  系统就绪检查（与 init_position_callback 一致）
      bool use_sim_time = false;
      this->get_parameter_or("use_sim_time", use_sim_time, false);
      if (use_sim_time && this->get_clock()->now().nanoseconds() == 0) return;
      if (this->host_publisher_->get_subscription_count() == 0 ||
          this->follow_publisher_->get_subscription_count() == 0) return;
      if (!this->got_joint_state_once_) return;

      // 取将要发布的值（未提供的项用当前/默认）
      double j5 = pending_j5_.value_or(this->joint5_position);
      double j6 = pending_j6_.value_or(this->joint6_right_position);
      double move_time = get_parameter("move_time").as_double();
      double start_delay = get_parameter("start_delay").as_double();
      
      // 范围校验（与服务口径一致）
      if (j5 < -2.36 || j5 > 2.36) {
        RCLCPP_WARN(this->get_logger(), "Param-triggered: joint5_position out of range [-2.36, 2.36]");
        // 清空缓存，避免反复尝试
        pending_j5_.reset(); pending_j6_.reset();
        this->joint_param_apply_once_timer_->cancel();
        return;
      }
      if (j6 < 0.0 || j6 > 1.58) {
        RCLCPP_WARN(this->get_logger(), "Param-triggered: joint6_right_position out of range [0, 1.58]");
        pending_j5_.reset(); pending_j6_.reset();
         this->joint_param_apply_once_timer_->cancel();
        return;
      }

       // 统一发布（与服务同路径）
      this->publishSynced(j5, j6, move_time, start_delay);
      RCLCPP_INFO(this->get_logger(), "Applied param-triggered gripper cmd: j5=%.3f, j6=%.3f", j5, j6);

      // 清空参数，并结束once timer
      pending_j5_.reset(); pending_j6_.reset();
      this->joint_param_apply_once_timer_->cancel();


    },this->cbgroup_one_); //最后的this->cbgroup_one_ 是将定时器加入互斥回调组，防止服务和定时器并发互踩
  }

  //一次性定时，等系统初始化好之后在发布初始位置的回调函数
  void init_position_callback(){
    bool use_sim_time = false;
    this->get_parameter_or("use_sim_time",use_sim_time,false);
    // 检查 /clock 是否开始跑了
    if(use_sim_time && this->get_clock()->now().nanoseconds() == 0)return ;
    // 检查控制器是否已经订阅
    if(this->host_publisher_->get_subscription_count() == 0 || this->follow_publisher_->get_subscription_count() == 0)return ;
    // 检查是否收到过一次joint_states
    if(!this->got_joint_state_once_) return ;

    //条件满足开始初始化position，start_delay给足一点
    this->publishSynced(0.0,0.0,1.0,0.5);
    RCLCPP_INFO(this->get_logger(), "InitGripper sent after system ready.");
    // 初始化后，定时结束
    this->init_postion_timer_->cancel();
  }

  // 订阅角度话题的回调函数
  void joint_listener_callback(const JointState::SharedPtr msg){
    int count = 0;
    for(size_t i = 0;i < msg->name.size();i++){
      if(msg->name[i] == "joint5"){
        this->joint5_position = msg->position[i];
        count++;
        if(count == 2)break;
      }
      if(msg->name[i] == "joint6_right"){
        this->joint6_right_position = msg->position[i];
        count++;
        if(count == 2)break;
      }
    }
  }

  // 设置参数回调函数，当在设置参数的时候调用
  rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector<rclcpp::Parameter> & params){
    rcl_interfaces::msg::SetParametersResult result;
    result.successful = true;
    result.reason = "success";
    for(const auto & p : params){
      if(p.get_name() == "open_angle"){
        double v = p.as_double();
        if(v < 0.0 || v > 1.58){
          result.successful = false;
          result.reason = "open_angle must be between 0 and 1.58 rad";
          RCLCPP_WARN(this->get_logger(),"open_angle must be between 0 and 1.58 rad");
        }
        else{
          RCLCPP_INFO(this->get_logger(), "Updated open_angle=%.2f", v);
        }
      }

      if(p.get_name() == "close_angle"){
        double v = p.as_double();
        if(v < 0.0 || v > 1.58){
          result.successful = false;
          result.reason = "close_angle must be between 0 and 1.58 rad";
          RCLCPP_WARN(this->get_logger(),"close_angle must be between 0 and 1.58 rad");
        }
        else{
          RCLCPP_INFO(this->get_logger(), "Updated close_angle=%.2f", v);
        }
      }

      if(p.get_name() == "start_delay"){
        double v = p.as_double();
        if(v < 0.0){
          result.successful = false;
          result.reason = "start_delay must be bigger than 0 seconds";
           RCLCPP_WARN(this->get_logger(),"start_delay must be bigger than 0 seconds");
        }
        else{
          RCLCPP_INFO(this->get_logger(), "Updated start_delay=%.2f", v);
        }
      }

      if(p.get_name() == "move_time"){
        double v = p.as_double();
        if(v < 0.0){
          result.successful = false;
          result.reason = "move_time must be bigger than 0 seconds";
          RCLCPP_WARN(this->get_logger(),"move_time must be bigger than 0 seconds");
        }
        else{
          RCLCPP_INFO(this->get_logger(), "Updated move_time=%.2f", v);
        }
      }
    }

    return result;
  }

  //将运动历经时间转换为 接口中类型
  static builtin_interfaces::msg::Duration secsToDuration(double s) {
    builtin_interfaces::msg::Duration d;
    if (s < 0) s = 0;
    // floor 向下取整 static_cast 强转
    d.sec = static_cast<int32_t>(std::floor(s));
    d.nanosec = static_cast<uint32_t>((s - std::floor(s)) * 1e9);
    return d;
  }

  void publishSynced(double host_joint5, double host_joint6_right, double move_time, double start_delay){
    // 先获取rclcpp::Time类型的开始时间
    const auto target_time = this->get_clock()->now() + rclcpp::Duration::from_seconds(start_delay);

    // 将其转化为接口类型的开始时间
    builtin_interfaces::msg::Time t0;
    t0.sec = static_cast<int32_t>(target_time.nanoseconds() / 1000000000);
    t0.nanosec = static_cast<uint32_t>(target_time.nanoseconds() % 1000000000);

    // 获取参数
    auto host_names   = get_parameter("host_joint_names").as_string_array();
    auto follow_names = get_parameter("follow_joint_names").as_string_array();
    auto scale        = get_parameter("follower_scale").as_double_array();
    auto offset       = get_parameter("follower_offset").as_double_array();

    // host 轨迹
    JointTrajectory host;
    host.header.stamp = t0;
    host.joint_names = host_names;

    JointTrajectoryPoint hp;
    hp.time_from_start = secsToDuration(move_time);
    hp.positions.resize(host_names.size(),0.0);
    for(size_t i = 0;i < host_names.size();i++){
      if (host_names[i] == "joint5") hp.positions[i] = host_joint5;
      else if (host_names[i] == "joint6_right") hp.positions[i] = host_joint6_right;
    }
    host.points.push_back(hp);

    // follower 轨迹 —— 镜像 joint6_right（附线性变换）
    JointTrajectory follower;
    follower.header.stamp = t0;
    follower.joint_names = follow_names;

    JointTrajectoryPoint fp;
    fp.time_from_start = secsToDuration(move_time);
    fp.positions.resize(follow_names.size(),0.0);
    for (size_t i=0;i<follow_names.size();++i) {
      double s = (scale.size()==follow_names.size()) ? scale[i] : 1.0;
      double b = (offset.size()==follow_names.size()) ? offset[i] : 0.0;
      fp.positions[i] = s * host_joint6_right + b;
    }
    follower.points.push_back(fp);

    this->host_publisher_->publish(host);
    this->follow_publisher_->publish(follower);
    RCLCPP_INFO(this->get_logger(), "Published synced gripper trajectories: t0=now+%.3fs, T=%.3fs", start_delay, move_time);
  }

  // 设置夹爪位置的回调函数
  void SetGripperPosition(const SetGripper::Request::SharedPtr request,SetGripper::Response::SharedPtr response){
    double move_time  = request->duration > 0.0 ?request->duration : get_parameter("move_time").as_double(); //获取运动历经
    double start_delay = get_parameter("start_delay").as_double();
    double joint5_angle = request->joint5_position;
    double joint6_right_angle = request->joint6_right_position;

    // 检查角度大小
    if(joint5_angle < -2.36 || joint5_angle > 2.36){
      response->result = SetGripper::Response::FAIL;
      response->message = "joint5_angle should be between -2.36 and 2.36";
      RCLCPP_WARN(this->get_logger(),"joint5_angle should be between -2.36 and 2.36");
      return ;
    }
    if(joint6_right_angle < 0 || joint6_right_angle > 1.58){
      response->result = SetGripper::Response::FAIL;
      response->message = "joint6_right_angle should be between 0 and 1.58";
      RCLCPP_WARN(this->get_logger(),"joint6_right_angle should be between 0 and 1.58");
      return ;
    }


    this->publishSynced(joint5_angle,joint6_right_angle,move_time,start_delay);

    response->result = SetGripper::Response::SUCESS;
    response->message = "gripper set ok";
    RCLCPP_INFO(this->get_logger(),"gripper set ok");
    
  }
  //默认开启夹爪的回调函数
  void OpenGripper(const Trigger::Request::SharedPtr request,Trigger::Response::SharedPtr response){
    //消除未使用警告
    (void)request;
    double host5 = 0.0;
    double move_time = get_parameter("move_time").as_double();
    double start_delay = get_parameter("start_delay").as_double();
    double open_a = get_parameter("open_angle").as_double();
    publishSynced(host5, open_a, move_time, start_delay);
    response->success = true;
    response->message = "gripper opened";
    RCLCPP_INFO(this->get_logger(),"gripper opened");
  }
  //默认关闭夹爪的回调函数
  void CloseGripper(const Trigger::Request::SharedPtr request,Trigger::Response::SharedPtr response){
    (void)request;
    double host5 = 0.0;
    double move_time = get_parameter("move_time").as_double();
    double start_delay = get_parameter("start_delay").as_double();
    double close_a = get_parameter("close_angle").as_double();
    publishSynced(host5, close_a, move_time, start_delay);
    response->success = true;
    response->message = "gripper closed";
    RCLCPP_INFO(this->get_logger(),"gripper closed");
  }
};

int main(int argc,char** argv){
  rclcpp::init(argc,argv);
  auto node = std::make_shared<GripperServer>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}